/*
 * main.cpp
 *
 *  Created on: Nov 17, 2012
 *      Author: bouchier
 */
/*! \file ROSIF/main.cpp
 * \brief Header file for ROSIF - the file which does whole-system exercising of jRover
 *
 * <H1>
 * Build Configuration
 * </H1>
 *
 * This project depends on the following peer projects being at the same directory level:
 * - Metro
 * - qetime
 * - ControlledMotor
 * - CQEI2C
 * - CQIMEncoder
 * - PID
 * - JRoverMotors
 * - JRoverSensors
 * - usonicRanger
 *
 * Edit the project properties as follows to reference them as includes, link objects, and referenced projects.
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Compiler group, Directories settings
 * add the following paths
 * to the terkos paths that are already there. This adds them to the include path for compilation
 * - ../../ros_lib
 * - ../../Metro
 * - ../../ControlledMotor
 * - ../../CQEI2C
 * - ../../JRoverMotors
 * - ../../JRoverSensors
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Linker group, Miscellaneous settings, add
 * the following "other objects". This tells the linker to link to qetime.o & CQEI2C.o.
 * - ../../Metro/Debug/Metro.o
 * - ../../ControlledMotor/Debug/ControlledMotor.o
 * - ../../CQEI2C/Debug/CQEI2C.o
 * - ../../CQIMEncoder/Debug/CQIMEncoder.o
 * - ../../PID/Debug/pid.o
 * - ../../JRoverMotors/Debug/JRoverMotors.o
 * - ../../JRoverSensors/Debug/JRoverSensors.o
 * - ../../usonicRanger/Debug/usonicRanger.o
 * - ../../RCTest/Debug/RCTest.o
 *
 * In the Project References group, check the following projects. This builds them before the current project.
 * CQEI2C
 * Metro
 * qetime
 * ControlledMotor
 * CQEI2C
 * CQIMEncoder
 * PID
 * JRoverMotors
 * JRoverSensors
 * usonicRanger
 * RCTest
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <qegpioint.h>
#include <qeanalog.h>
#include "ROSIF.h"
#include <qetime.h>
#include <CQEI2C.h>
#include <ControlledMotor.h>
#include <JRoverMotors.h>
#include <JRoverSensors.h>

#define DEFAULT_TEST 3
ROSIF rosif = ROSIF();
JRoverMotors jRoverMotors = JRoverMotors();
JRoverSensors jRoverSensors = JRoverSensors();

CQEGpioInt &gpio = CQEGpioInt::GetRef();
CQEAnalog &analog = CQEAnalog::GetRef();
CQEI2C i2c = CQEI2C();		// instantiate the I2C driver

#define PI 3.14159265359
#define WHEEL_CIRCUMFERENCE 2 * PI * 2

void test1()
{
	printf("Running test 1\n");

	jRoverSensors.initSensors();

	if (!rosif.initRos()) {
		printf("ERROR: Failed to initialize rosserial\n");
		return;
	}
	while (1) {
		rosif.poll();
		sleep(1);
	}
}

void test2()
{
	float f = 0.0;

	printf("Running test 2\n");

	if (!rosif.initRos()) {
		printf("ERROR: Failed to initialize rosserial\n");
		return;
	}

	rosif.telemetryTestFlag = true;

	while(1){
		rosif.publishDriveMotorTelemetry();
		rosif.publishSteerMotorTelemetry();
		rosif.setTestRobotMotionData(f);
		rosif.publishRobotTelemetry();
		rosif.poll();
		f += 1.0;
		sleep(1);
	}
}

void test3()
{
	printf("Running test 3\n");
	float rosLinear, rosAngular;	// values from twist message

	if (!rosif.initRos()) {
		printf("ERROR: Failed to initialize rosserial\n");
		return;
	}

	// not testing
	rosif.telemetryTestFlag = false;
	jRoverSensors.initSensors();

	while(1){
		// ask the system controllers to fill in telemetry data into the ROS msg
		jRoverMotors.getDriveMotorsTelemetry();
		jRoverMotors.getSteerMotorsTelemetry();
		jRoverSensors.pollSensors();

		// now publish it
		rosif.publishDriveMotorTelemetry();
		rosif.publishSteerMotorTelemetry();
		rosif.publishRobotTelemetry();
		rosif.poll();

		// read ros speed request
		rosif.getRosLinearAngular(rosLinear, rosAngular);
		sleep(1);
	}
}

void usage()
{
	printf("Usage: ROSIF testNum\n");
	printf("testNum can be:\n");
	printf("1: connect to ROS\n");
	printf("2: publish drive, steer, robot test values\n");
	printf("3: publish real drive, steer, robot values\n");
}

int main(int argc, char **argv)
{
 	int test = DEFAULT_TEST;	// which test to run

	if (argc == 2)
		test = atoi(argv[1]);

	switch (test) {
	case 1: test1(); break;
	case 2: test2(); break;
	case 3: test3(); break;
	default:
		printf("Invalid option\n");
		usage();
	}
}


